function varargout = inverseKine(robot,T,q0,eps)
    % 超冗余机械臂逆解(Levenberg-Marquardt算法（收敛较快）、Newton-Raphson算法（收敛适中）、Gradient Descent算法（收敛过慢）)
    % robot：机器人SerialLink对象
    % T：目标位姿
    % q0：初始位置(猜测值)
    % eps：精度要求
    % lrm：步长最大值(不同机器人不一定相同)
    % lrn：步长最小值(不同机器人不一定相同)
    % max_iter：最大迭代次数(不同机器人不一定相同)
    % 优化算法得出LM最佳值
    % lrm  = 0.0129301727204799;
    % lrn = 0.0108380953109218;
    % 优化算法得出NR最佳值
    lrm = 1.63372391430955;
    lrn = 0.0926739997018773;
    max_iter = 500;
    if 0 >= eps
        disp(eps)
        error("eps input error")
    end
    q_k_1 = q0;
    % 计算初始末端位姿误差
    X_d = Trans2X(T);
    X_k_1 = Trans2X(robot.fkine(q_k_1));
    err_X = X_d - X_k_1;
    err = norm(err_X,2);
    err_log = [];
    iter = 0;
    % % Levenberg-Marquardt算法
    % while iter < max_iter && err > eps
    %     % 几何雅可比矩阵
    %     J_k_1 = robot.jacob0(q_k_1);
    %     % 欧拉角速度转换到姿态角速度之间的雅可比矩阵的逆矩阵
    %     sa = sin(X_k_1(4));ca = cos(X_k_1(4));
    %     sb = sin(X_k_1(5));cb = cos(X_k_1(5));
    %     J_Eular_inv = [ca*sb/cb sa*sb/cb 1;
    %                     -sa ca 0;
    %                     ca/cb sa/cb 0];
    %     % 几何雅可比矩阵转换到分析雅可比
    %     J_g2a = [eye(3) zeros(3,3);
    %             zeros(3,3) J_Eular_inv];
    %     J_a_k_1 = J_g2a*J_k_1;
    %     % 余弦变化学习率
    %     % lr1 = lrn + 0.5*(lrm-lrn)*(1+cos(iter/max_iter*pi));
    %     lr = lrn + 0.5*(lrm-lrn)*(1+cos((max_iter - iter)/max_iter*pi));
    %     % 逆运动学数值更新公式
    %     q_k = q_k_1 + ((J_a_k_1'*J_a_k_1+lr*eye(length(q0)))\J_a_k_1'*err_X)';
    %     q_k = jlim(q_k,robot.qlim);% 关节限位
    %     q_k_1 = q_k;
    %     % 计算当前末端位姿误差
    %     X_k_1 = Trans2X(robot.fkine(q_k_1));
    %     err_X = X_d - X_k_1;
    %     err = norm(err_X,2);
    %     err_log = [err_log;err];
    %     % 迭代次数
    %     iter = iter + 1;
    % end

    % Newton-Raphson算法
    while iter < max_iter && err > eps
        % 几何雅可比矩阵
        J_k_1 = robot.jacob0(q_k_1);
        % 欧拉角速度转换到姿态角速度之间的雅可比矩阵的逆矩阵
        sa = sin(X_k_1(4));ca = cos(X_k_1(4));
        sb = sin(X_k_1(5));cb = cos(X_k_1(5));
        J_Eular_inv = [ca*sb/cb sa*sb/cb 1;
                        -sa ca 0;
                        ca/cb sa/cb 0];
        % 几何雅可比矩阵转换到分析雅可比
        J_g2a = [eye(3) zeros(3,3);
                zeros(3,3) J_Eular_inv];
        J_a_k_1 = J_g2a*J_k_1;
        % 余弦变化学习率
        lr = lrn + 0.5*(lrm-lrn)*(1+cos(iter/max_iter*pi));
        % 逆运动学数值更新公式
        q_k = q_k_1 + (lr*pinv(J_a_k_1)*err_X)';
        q_k = jlim(q_k,robot.qlim);% 关节限位
        q_k_1 = q_k;
        % 计算当前末端位姿误差
        X_k_1 = Trans2X(robot.fkine(q_k_1));
        err_X = X_d - X_k_1;
        err = norm(err_X,2);
        err_log = [err_log;err];
        % 迭代次数
        iter = iter + 1;
    end

    % % Gradient Descent算法（收敛过慢）
    % while iter < max_iter && err > eps
    %     % 几何雅可比矩阵
    %     J_k_1 = robot.jacob0(q_k_1);
    %     % 欧拉角速度转换到姿态角速度之间的雅可比矩阵的逆矩阵
    %     sa = sin(X_k_1(4));ca = sin(X_k_1(4));
    %     sb = sin(X_k_1(5));cb = sin(X_k_1(5));
    %     J_Eular_inv = [ca*sb/cb sa*sb/cb 1;
    %                     -sa ca 0;
    %                     ca/cb sa/cb 0];
    %     % 几何雅可比矩阵转换到分析雅可比
    %     J_g2a = [eye(3) zeros(3,3);
    %             zeros(3,3) J_Eular_inv];
    %     J_a_k_1 = J_g2a*J_k_1;
    %     % 余弦变化学习率
    %     lr = lrn + 0.5*(lrm - lrn)*(1+cos(iter/max_iter*pi));
    %     % 逆运动学数值更新公式
    %     q_k = q_k_1 + (lr*J_a_k_1'*err_X)';
    %     if mod(iter,50)==0
    %         disp((lr*J_a_k_1'*err_X)')
    %     end
    %     q_k = jlim(q_k,robot.qlim);% 关节限位
    %     q_k_1 = q_k;
    %     % 计算当前末端位姿误差
    %     X_k_1 = Trans2X(robot.fkine(q_k_1));
    %     err_X = X_d -  X_k_1;
    %     err = norm(err_X,2);
    %     err_log = [err_log;err];
    %     % 迭代次数
    %     iter = iter+1;
    % end
    if iter >= max_iter
        warning("达到最大迭代次数");
    end
    if nargout == 1
        varargout{1} = q_k_1;
    elseif nargout == 2
        varargout{1} = q_k_1;
        varargout{2} = iter;
    elseif nargout == 3
        varargout{1} = q_k_1;
        varargout{2} = iter;   
        varargout{3} = err_log;
    else
        error('Too many output argment!')
    end
    % 齐次变换矩阵转换到末端位置最小表达
    function X = Trans2X(T)
        X = zeros(6,1);
        X(1:3) = T.t;
        eular = rotm2eul([T.n T.o T.a],'zyx');
        X(4:6) = eular'; 
    end
    % 关节限位
    function q = jlim(q,qlim)
        % size(qlim) = (n,2);
        % 向量格式统一
        if any(size(q) ~= size(qlim(:,1)))
            qmax = qlim(:,2)';
            qmin = qlim(:,1)';
        else
            qmax = qlim(:,2);
            qmin = qlim(:,1);
        end
        % 大小限制
        index = q > qmax;
        q(index) = qmax(index);
        index = q < qmin;
        q(index) = qmin(index);
    end
end